/*
   This program uses interrupts for (longer) timing tasks in the seconds range.

   @see: http://brettbeauregard.com/blog/2011/04/improving-the-beginner%e2%80%99s-pid-sample-time/
   @see: https://arduino-projekte.webnode.at/registerprogrammierung/timer-interrupt/
   @see: http://www.gammon.com.au/interrupts
   @see: http://www.gammon.com.au/timers
   @see: http://www.instructables.com/id/Arduino-Timer-Interrupts/
   @see: http://maxembedded.com/2011/07/avr-timers-ctc-mode/
 * */

// if _DEBUG0_ is defined important errors about getter and setter are written to serial.
// if _DEBUG1_ is defined detailed output about the state of the controller is written to serial.
#define _DEBUG0_
//#define _DEBUG1_
//#undef _DEBUG0_
//#undef _DEBUG1_

////////////////////////////////////////////////////////////////////////////////////////////
// MAX31855 k-type thermocouple:
//                creating a thermocouple instance with hardware SPI on a given CS pin.
////////////////////////////////////////////////////////////////////////////////////////////
#include "Adafruit_MAX31855.h"
#include "Commands.h"
#define MAXCS   10 // = SPI:SS = D10
Adafruit_MAX31855 thermocouple(MAXCS);

////////////////////////////////////////////////////////////////////////////////////////////
// PID
////////////////////////////////////////////////////////////////////////////////////////////
#include "PIDcontrol.h"

// set the pins for input, output and sample frequency here:
const unsigned short PID_RELAY_PIN = 5; // pin D5 on Arduino Nano that is used to control relay

// specify the PID values here:
double pValue = 1000;                   // the proportional value
double iValue = 0;                      // the integral value
double dValue = 0;                      // the derivative value

double pidSetpoint;                     // the desired target setpoint
volatile double processValue;           // the input to the PID controller as read from the sensor
volatile double lastProcessValue;       // the last input to be remembered in case the current reading is NAN (Not a Number) and cannot be used for computation.
volatile double controlValue;           // specifies how long the relay is switched on within a fixed period of time (fraction of sampleInterval), this controls how long the thermo element is switched on
unsigned short  sampleInterval;         // a fixed period of time that determines a full PID on/off cycle (ms) ATTENTION: sampleInterval [ms] is the same as INTERRUPT_INTERVAL[s] and cannot be greater than 4.1944 seconds
volatile bool   errorReading;           // is set when temperature reading returned NAN
// initialize the PID controller
PIDcontrol myPID(&processValue, &controlValue, &pidSetpoint, pValue, iValue, dValue, DIRECT);

////////////////////////////////////////////////////////////////////////////////////////////
// INTERRUPT
////////////////////////////////////////////////////////////////////////////////////////////

// ATTENTION: Here TIMER 1 is used which is a 16-bit timer. Thus the compare value cannot be greater than 65535, otherwise the timer would overflow.
//            So the maximum time between two interrupts with a cpu frequency of 16000000 hz can only be:
//                OCR1A = CMP_VALUE = INTERRUPT_INTERVAL * TICKS_PER_SEC - 1
//                => INTERRUPT_INTERVAL = CMP_VALUE / (TICKS_PER_SEC - 1)
//                => INTERRUPT_INTERVAL = CMP_VALUE / (( CPU_FREQ / PRESCALER ) - 1)
//                => INTERRUPT_INTERVAL = 65535 / (( 16000000 / 1024 ) - 1)
//                => INTERRUPT_INTERVAL = 4.1945 seconds

#define CPU_FREQ 16000000L    // cpu clock from boards.txt
#define PRESCALER 1024        // cpu prescaler

// NOTE: we use seconds here as a basis because when using miliseconds the precision of the number would be inexact due to floating point calculations.
const unsigned short TICKS_PER_SEC = ( CPU_FREQ / PRESCALER );            // base for calculation of compare value for output compare register OCR1A
const unsigned short INTERRUPT_INTERVAL = 4;                              // interrupt every 4 seconds (=maximum interrupt intervall and maximum sample pid sample time)
const unsigned short CMP_VALUE = INTERRUPT_INTERVAL * TICKS_PER_SEC - 1;  // compare value for OCR1A: = amount of ticks in an interrupt intervall
const unsigned short MAX_CMP_VALUE = 65535 - 1;                           // maximum number in a 16-bit variable

String serialCmd;
CommandId serialCmdId;
double serialParam;
volatile unsigned long counter = 0;
volatile unsigned long cntCmp = 0;

////////////////////////////////////////////////////////////////////////////////////////////
// SETUP
////////////////////////////////////////////////////////////////////////////////////////////
void setup()
{
    // MAX31555 thermocouple and serial initialization
    Serial.begin(115200);
    while (!Serial) delay(1);           // wait for serial on Leonardo/Zero, etc
    delay(500);                         // wait for MAX chip to stabilize
    // SSR initialization
    pinMode(PID_RELAY_PIN, OUTPUT);     // set the relay pin to output
    digitalWrite(PID_RELAY_PIN, LOW);   // initialize relay pin to low
    //PID
    pidSetpoint = 55;                            // set the desired target value (=55° C)
    sampleInterval = INTERRUPT_INTERVAL * 1000;  // the sample time of the pid controller is the interrupt frequency in miliseconds
    myPID.SetSampleTime(sampleInterval);         // tell the PID controller how frequently we will read a sample and calculate output
    myPID.SetOutputLimits(0, sampleInterval);    // tell the PID to range between 0 and the full window size
    myPID.SetMode(AUTOMATIC);                    // turn the PID on
    processValue = thermocouple.readCelsius();       // read temperature sensor input
    myPID.Compute();                             // compute controlValue for the first time

    Serial.print("CPU_FREQ:            ");
    Serial.println(CPU_FREQ);
    Serial.print("PRESCALER:           ");
    Serial.println(PRESCALER);
    Serial.print("TICKS_PER_SEC:       ");
    Serial.println(TICKS_PER_SEC);
    Serial.print("CMP_VALUE:           ");
    Serial.println(CMP_VALUE);
    Serial.print("MAX_CMP_VALUE:       ");
    Serial.println(MAX_CMP_VALUE);
    Serial.print("INTERRUPT_INTERVAL: ");
    Serial.println(INTERRUPT_INTERVAL);

    Serial.print("sampleInterval: ");
    Serial.print(sampleInterval);
    Serial.print(" | Setpoint: ");
    Serial.print(pidSetpoint);
    Serial.print(" | P: ");
    Serial.print(pValue);
    Serial.print(" I: ");
    Serial.print(iValue);
    Serial.print(" D: ");
    Serial.println(dValue);

    // INTERRUPTS
    cli();                                      // disable global interrupts
    TCNT1 = 0;                                  // delete timer counter register
    TCCR1A = 0;                                 // delete TCCR1A-Registers
    TCCR1B = 0;                                 // delete TCCR1B-Registers
    TCCR1B |= (1 << WGM12);                     // CTC-Mode (Waveform Generation Mode): resets TCNT1 to0 after interrupt, makes OCR1A the leading compare register
    TCCR1B |= (1 << CS12) | (1 << CS10);        // set prescaler to 1024: CS10 und CS12 (Clock Select)
    OCR1A = CMP_VALUE;                          // set compare value
    OCR1B = MAX_CMP_VALUE;                      // a second comapare value in OCR1B can be used as long as its value is *lower* than that of OCR1A: here its higher so it wont be called in the beginning
    TIMSK1 |= (1 << OCIE1A) | (1 << OCIE1B);    // enable interrupts: set output compare interrupt enable for 1A and 1B
    sei();                                      // enable global interrupts
}

////////////////////////////////////////////////////////////////////////////////////////////
// INTERRUPTS
////////////////////////////////////////////////////////////////////////////////////////////
ISR(TIMER1_COMPA_vect)   // Interrupt Service Routine for timer 1 OCR1A
{
    processValue = thermocouple.readCelsius(); // read temperature sensor input
    if (!isnan(processValue)) lastProcessValue = processValue;
    else
    {
        processValue = lastProcessValue;
        errorReading = true;
    }
    myPID.Compute();                       // compute controlValue

    if (controlValue > 0)
        digitalWrite(PID_RELAY_PIN, HIGH);
    else
        digitalWrite(PID_RELAY_PIN, LOW);

    // trigger interrupt b to turn off relay within the sample time (=sampleInterval)
    if ( controlValue > 0 && controlValue < sampleInterval )
        OCR1B = ((controlValue * TICKS_PER_SEC) / 1000) - 1;
    else
        OCR1B = MAX_CMP_VALUE;    // we set OCR1B higher than OCR1A so that the second b interrupt never gets called

    ++counter;                    // flag that signals the start of a new cycle to the main loop
}

// Interrupt Service Routine for timer 1 OCR1B
ISR(TIMER1_COMPB_vect)
{
    digitalWrite(PID_RELAY_PIN, LOW);
}

////////////////////////////////////////////////////////////////////////////////////////////
// MAIN LOOP
////////////////////////////////////////////////////////////////////////////////////////////
void loop()
{
    if (Serial.available()) 
    {
        serialCmd = Serial.readString();
        serialCmdId = (CommandId) serialCmd[0];
        
        if (GETTERSTART < serialCmdId && serialCmdId < SETTERSTART)
        {
            returnSetting();            
        } 
        else if (SETTERSTART < serialCmdId && serialCmdId < SETTEREND)
        {
            if (serialCmd.length() > 1)
            {
                serialParam = serialCmd.substring(1).toDouble();
                setValues();
            } 
            #if defined _DEBUG0_
                else
                {
                    Serial.print("Missing parameter for \"enum serialCmdId = ");
                    Serial.print(serialCmdId);
                    Serial.println("\" (see Commands.h)");
                }
            #endif // _DEBUG0_
        }
        #if defined _DEBUG0_
            else 
            {
                Serial.print("ERROR: invalid commandId \"enum serialCmdId = ");
                Serial.print(serialCmdId);
                Serial.println("\" (see Commands.h)");
            }
        #endif // _DEBUG0_

        /*if (serialCmd == "getP")                              // Getter
            Serial.println(pValue);
        else if (serialCmd == "getI")
            Serial.println(iValue);
        else if (serialCmd == "getD")
            Serial.println(dValue);
        else if (serialCmd == "getProcessValue")
            Serial.println(processValue);
        else if (serialCmd == "getControlValue")
            Serial.println(controlValue);
        else if (serialCmd == "getSetpoint")
            Serial.println(pidSetpoint);
        else if (serialCmd == "getSampleInterval")
            Serial.println(sampleInterval);
        else if (serialCmd == "setP")                         // Setter
            Serial.println(pValue);
        else if (serialCmd == "setI")
            Serial.println(iValue);
        else if (serialCmd == "setD")
            Serial.println(dValue);
        else if (serialCmd == "setSetpoint")
            Serial.println(pidSetpoint);
        else if (serialCmd == "setSampleInterval")
            Serial.println(sampleInterval);
        else
            Serial.println("ERROR: unknown command \"" + serialCmd + "\"");*/
    }   
     
    #if defined _DEBUG1_
        if (cntCmp != counter)    // use != for comparison and not < because it could give problems when unsigned long overflows
        {
            Serial.print(counter);
            Serial.print(": ");
            Serial.print(millis());
            if (errorReading)   // for error codes see: https://cdn-shop.adafruit.com/datasheets/MAX31855.pdf#page=10 
            {
                Serial.print(":\tERROR: ");
                uint8_t errorCode = thermocouple.readError();
                switch( errorCode )
                {
                case 1:
                    Serial.println("(1) Thermocouple has no connection (open circuit)!");
                    break;
                case 2:
                    Serial.println("(2) Thermocouple short-circuited to GND!");
                    break;
                case 4:
                    Serial.println("(4) Thermocouple short-circuited to VCC!");
                    break;
                default:
                    Serial.print("(");
                    Serial.print(errorCode);
                    Serial.println(") Unkown error code!");
                }
                errorReading=false;    // IMPORTANT: reset error flag !!
            }
            else
            {
                Serial.print(":\tprocessValue: ");
                Serial.print( processValue);
                Serial.print(" | controlValue: ");
                Serial.print(controlValue);
                Serial.print(" | OCR1A: ");
                Serial.print(OCR1A);
                Serial.print(" | OCR1B: ");
                Serial.println(OCR1B);
            }
            cntCmp = counter;
        }
    #endif // _DEBUG1_
}

// getter
inline void returnSetting()
{
    switch(serialCmdId)
    {
    case getP:
        Serial.println(pValue);
        break;
    case getI:
        Serial.println(iValue);
        break;
    case getD:
        Serial.println(dValue);
        break;
    case getProcessValue:
        Serial.println(processValue);
        break;
    case getControlValue:
        Serial.println(controlValue);
        break;
    case getSetpoint:
        Serial.println(pidSetpoint);
        break;
    case getSampleInterval:
        Serial.println(sampleInterval);
        break;
    }
}

// setter
inline void setValues()
{
    switch(serialCmdId)
    {
    case setP:
        pValue = serialParam;
        myPID.SetTunings(pValue,iValue,dValue);
        //Serial.println(myPID.GetKp());
        break;
    case setI:
        iValue = serialParam;
        myPID.SetTunings(pValue,iValue,dValue);
        //Serial.println(myPID.GetKi());
        break;
    case setD:
        dValue = serialParam;
        myPID.SetTunings(pValue,iValue,dValue);
        //Serial.println(myPID.GetKd());
        break;
    case setSetpoint:
        pidSetpoint= serialParam;
        break;
    /*case setSampleInterval:
        sampleInterval = static_cast<unsigned short>(serialParam);
        myPID.SetSampleTime(sampleInterval);
        break;*/
    }
}
